import numpy as np
from numpy.linalg import norm, inv


def trustq(gk, Bk, dta):
    '''
    功能: 求解信赖域子问题: min qk(d)=gk'*d+0.5*d'*Bk*d, s.t. ||d||<=delta
    输入: gk是xk处的梯度, Bk是第k次近似Hesse阵, dta是当前信赖域半径
    输出: d, val分别是子问题的最优点和最优值, lam是乘子值, k是迭代次数.
    '''

    n = len(gk)
    gamma = 0.05
    epsilon = 1e-06
    rho = 0.6
    sigma = 0.2
    mu0 = 0.05
    lam0 = 0.05
    d0 = np.mat(np.ones((n, 1)))
    u0 = np.mat(np.append([mu0], np.zeros((1, n + 1)))).T
    z0 = np.mat(np.append([mu0, lam0], d0.T)).T
    k = 0  # 迭代次数
    z = z0
    mu = mu0
    lam = lam0
    d = d0
    while (k <= 150):

        dh = dah(mu, lam, d, gk, Bk, dta)
        if (norm(dh) < epsilon):
            break
        A = JacobiH(mu, lam, d, Bk, dta)
        b = beta(mu, lam, d, gk, Bk, dta, gamma) * u0 - dh
        B = inv(A)
        dz = B * b
        dmu = dz[0, 0]
        dlam = dz[1, 0]
        dd = dz[2:n + 2, 0]
        m = 0
        mk = 0
        while (m < 20):

            dhnew = dah(mu + rho ** m * dmu, lam + rho ** m * dlam, d + rho ** m * dd, gk, Bk, dta)
            if (norm(dhnew) <= ((1 - sigma * (1 - gamma * mu0) * rho ** m) * dh).all()):
                mk = m
                break
            m = m + 1

        alpha = rho ** mk
        mu = mu + alpha * dmu
        lam = lam + alpha * dlam
        d = d + alpha * dd
        k = k + 1

    val = gk.T * d + 0.5 * d.T * Bk * d
    return d, val, lam, k

def phi(mu,a,b):
    p = a + b - np.sqrt((a - b) ** 2 + 4 * mu)
    return p

def dah(mu,lam,d,gk,Bk,dta):
    n = len(d)
    dh = np.mat(np.zeros((n+2,1)))
    dh[0] = mu
    dh[1] = phi(mu,lam,dta ** 2 - norm(d) ** 2)
    mh = (Bk + lam * np.eye(n)) * d + gk
    mhf = mh.flatten()
    for i in range(n):
        dh[2+i] = mhf[0,i]

    return dh

def beta(mu,lam,d,gk,Bk,dta,gamma):
    dh = dah(mu,lam,d,gk,Bk,dta)
    bet = gamma * norm(dh) * min(1,norm(dh))
    return bet

def JacobiH(mu,lam,d,Bk,dta):
    n = len(d)
    A = np.mat(np.zeros((n + 2, n + 2)))
    pmu = -4 * mu / np.sqrt((lam + norm(d) ** 2 - dta ** 2) ** 2 + 4 * mu ** 2)
    thetak = (lam + norm(d) ** 2 - dta ** 2) / np.sqrt((lam + norm(d) ** 2 - dta ** 2) ** 2 + 4 * mu ** 2)
    a1 = np.append([1, 0], np.zeros((1,n)))
    a2 = np.append([pmu,1 - thetak],-2 * (1 + thetak) * d.T)
    nz = np.zeros((n,1))
    a3 = Bk + lam * np.eye(n)
    A[0] = np.mat(a1)
    A[1] = np.mat(a2)
    for i in range(n):
        A[2+i] = np.append([0, d[i, 0]], a3[i])

    return A

# 函数调用

# gk = np.mat([400,- 200]).T
# Bk = np.mat([[1202,- 400],[-400,200]])
# gk = np.mat([-6,-3]).T
# Bk = np.mat([[4,- 4],[-4,8]])
gk = np.mat([1,-3,-2]).T
Bk = np.mat([[3,-1,2],[-1,2,0],[2,0,4]])
dta = 1.108
d,val,lam,k = trustq(gk,Bk,dta)

print("最优点:",d,"最优解:", val,"\n乘子值:", lam,"迭代次数:" ,k)

